#include "MotorizedPrototype.hpp"
// https://github.com/nbsdx/SimpleJSON
#include "Libs/SimpleJSON/JSON.hpp"
#include "MiscUtils/TdrMisc/SEngine.h"
#include "Comms/SerialPortManager/SerialPortManager.hpp"

using namespace std;

bool MotorizedSlave::Axis::NomimalToPhysicalMap::has(float value, float errorAllowance) {
	for (auto& entry : entrys) {
		if (abs(entry.nominal - value) < errorAllowance) {
			return true;
		}
	}
	return false;
}

float MotorizedSlave::Axis::NomimalToPhysicalMap::get(float value, float errorAllowance) {
	for (auto& entry : entrys) {
		if (abs(entry.nominal - value) < errorAllowance) {
			return entry.physical;
		}
	}
	return 0;
}

void MotorizedSlave::Axis::NomimalToPhysicalMap::add(float nomimalIn, float physicalIn) {
	entrys.resize(entrys.size() + 1);
	entrys.back().nominal = nomimalIn;
	entrys.back().physical = physicalIn;
}

void MotorizedSlave::Axis::init() {
	//memset(axisMask, 0, sizeof(axisMask));
}

void MotorizedSlave::Axis::initSimu() {
	//axisMask[0] = 1;
	axisFuncName[0] = L"move_x";
	axisDirName[0][0] = L"left";
	axisDirName[0][1] = L"right";
	axisCtrlUnit[0] = L"mm";
	axisOutputUnit[0] = L"mm";
	axisOutputPerCtrlUnit[0] = 1;
	//axisMask[1] = 1;
	axisFuncName[1] = L"move_y";
	axisDirName[1][0] = L"back";
	axisDirName[1][1] = L"fore";
	axisCtrlUnit[1] = L"mm";
	axisOutputUnit[1] = L"mm";
	axisOutputPerCtrlUnit[1] = 1;
	//axisMask[2] = 1;
	axisFuncName[2] = L"move_z";
	axisDirName[2][0] = L"high";
	axisDirName[2][1] = L"low";
	axisCtrlUnit[2] = L"mm";
	axisOutputUnit[2] = L"mm";
	axisOutputPerCtrlUnit[2] = 1;
}

int MotorizedSlave::Axis::getDefinedAxisNum() {
	int maxIdx = 0;
	for (int idx = 0; idx < maxAxisNum; idx++) {
		if (axisFuncName[idx].length() > 0) {
			maxIdx = idx;
		}
	}
	return maxIdx + 1;
}

MotorizedSlave::MotorizedSlave() {
	//genSlaveInit();
	//_misc.slave = new GRBLMaster();

	//_meta.referenceName = L"SlideRackPlatform";
	//_meta.platform = L"Terrydr";
	//_meta.type = L"Robot";
	//_meta.model = L"SlideRackPlatform";
	//_meta.isExpecting = 1;
	//setDefaultFriendlyName();
	motorInit();
}

MotorizedSlave::MotorizedSlave(GeneralSlave* ref) {
	motorInit(ref);
}

MotorizedSlave::~MotorizedSlave() {
	genSlaveRelease();
	motorRelease();
}

int MotorizedSlave::motorInit(GeneralSlave* ref) {
	genSlaveInit(ref);
	setDefaultFriendlyName();

	//_comm.handShakePreSends.push_back("?");
	//_comm.handShakePreSends.back()[0] = 0x18;
	//_comm.handShakePreReceives.push_back("Grbl init\r\n");

	//_comm.handShakePreSends.push_back("$X\n");
	//_comm.handShakePreReceives.push_back("ok\r\n");

	_motorAxis.init();
	loadAxisDefinition(ref);
	//initCoreAndLinkVars();
	return 1;
}

void MotorizedSlave::motorInit() {
	_motorAxis.initSimu();
	genSlaveInit();

	//initCoreAndLinkVars();
	initDefaultMeta();
}

void MotorizedSlave::motorRelease() {
}

int MotorizedSlave::setOfflineSimu(int flag, float simuAccRateMMPerSecSec) {
	//_misc.slave;
	//SlaveNat->_flags.simuMode = (simuAccRateMMPerSecSec > 0);
	//SlaveNat->_flags.simuAccMMPerSecSec = simuAccRateMMPerSecSec;
	return -1;
}

int MotorizedSlave::home_able() {
	return _motorAxis.homeDirection != 0;
}

int MotorizedSlave::associatedSensorReadoutInIdle(uint64_t readIndex) {
	int ret = 1;
	for (int idx = 0; idx < _motorAxis.getDefinedAxisNum(); idx++) {
		if (_motorAxis.positionSensorOfAxis[idx] != nullptr) {
			//int nRet = _motorAxis.positionSensorOfAxis[idx]->readOutInIdle(readIndex);
			ret = min<int>(ret, _motorAxis.positionSensorOfAxis[idx]->readOutInIdle(readIndex));
		}
	}
	return ret;
}

int MotorizedSlave::miscFunc(std::wstring command, std::wstring& reply) {
	TLogger block("Misc");

	string cmdOut = TFormater::wstring2string(command);
	cmdOut = cmdOut + "\n";
	preSendingConfig();
	if (sendString(cmdOut) > 0) {
		string rep;
		if (receive(&rep) > 0) {
			for (int idx = rep.length() - 1; idx >= 0; idx--) {
				if (rep[idx] == '\r' || rep[idx] == '\n') {
					rep[idx] = '\0';
				}
				else {
					break;
				}
			}
			reply = TFormater::string2wstring(rep);
			return 1;
		}
		else {
			reply = L"Exception: time out";
			return 0;
		}
	}
	reply = L"Exception: cannot send";
	return 0;
}

void* MotorizedSlave::summarizeFunctionsJson() {
	JSONArray slaveFuncs;
	for (int idx = 0; idx < _motorAxis.getDefinedAxisNum(); idx++) {
		JSONObject funcObj;
		funcObj[L"name"] = new JSONValue(_motorAxis.axisFuncName[idx]);
		wstring options = _motorAxis.axisDirName[idx][0];
		if (_motorAxis.axisDirName[idx][1].length() > 0) {
			options += L"|" + _motorAxis.axisDirName[idx][1];
		}
		funcObj[L"options"] = new JSONValue(options);
		funcObj[L"unit"] = new JSONValue(_motorAxis.axisOutputUnit[idx]);
		funcObj[L"is_axial"] = new JSONValue(idx);
		funcObj[L"ex_axial"] = new JSONValue(-1);
		if (_motorAxis.axisSelectableNum > 0) {
			funcObj[L"synced_axis_group"] = new JSONValue(0);
		}
		else {
			funcObj[L"synced_axis_group"] = new JSONValue(-1);
		}
		slaveFuncs.push_back(new JSONValue(funcObj));

		if (_motorAxis.axisExFuncCode[idx].length() > 0) {
			// ex functions
			auto funcs = TWFormater::split(_motorAxis.axisExFuncCode[idx], L',');
			for (auto& func : funcs) {
				wstring options;
				if (TWFormater::between(func, L"valve={", L"}", options)) {
					JSONObject funcObj;
					funcObj[L"name"] = new JSONValue(L"valve");
					funcObj[L"options"] = new JSONValue(options);
					funcObj[L"unit"] = new JSONValue(L"");
					funcObj[L"is_axial"] = new JSONValue(-1);
					funcObj[L"ex_axial"] = new JSONValue(idx);
					slaveFuncs.push_back(new JSONValue(funcObj));
				}
				else if (TWFormater::between(func, L"valves={", L"}", options)) {
					auto multiControl = func.substr(func.find_last_of(L'}') + 2);
					auto chNum = TWFormater::str2int(multiControl);
					JSONObject funcObj;
					funcObj[L"name"] = new JSONValue(L"valves");
					funcObj[L"options"] = new JSONValue(options);
					funcObj[L"unit"] = new JSONValue(L"");
					funcObj[L"is_axial"] = new JSONValue(-1);
					funcObj[L"ex_axial"] = new JSONValue(idx);
					wstring codeStrs;
					for (int idx = 0; idx < chNum; idx++) {
						wstring codeStr = TWFormater::int2str(idx);
						TWFormater::append(codeStrs, codeStr, L",");
					}
					funcObj[L"codes"] = new JSONValue(codeStrs);
					slaveFuncs.push_back(new JSONValue(funcObj));
				}
			}
		}
	}
	for (int idx = 0; idx < _motorMiscFunctions.funcs.size(); idx++) {
		auto& funcGroup = _motorMiscFunctions.funcs[idx];
		JSONObject funcObj;
		funcObj[L"name"] = new JSONValue(funcGroup.name);
		wstring options;
		options += funcGroup.calls[0];
		for (int opIdx = 1; opIdx < funcGroup.calls.size(); opIdx++) {
			options += L"|" + funcGroup.calls[opIdx];
		}
		funcObj[L"options"] = new JSONValue(options);
		funcObj[L"unit"] = new JSONValue(funcGroup.unit);
		funcObj[L"is_axial"] = new JSONValue(-1);
		funcObj[L"ex_axial"] = new JSONValue(-1);
		slaveFuncs.push_back(new JSONValue(funcObj));
	}
	return new JSONValue(slaveFuncs);
}

void* MotorizedSlave::summarizeFunctionsJsonListed(map<wstring, int> list) {
	// not checked
	JSONArray slaveFuncs;
	for (int idx = 0; idx < _motorAxis.getDefinedAxisNum(); idx++) {
		JSONObject func;
		func[L"name"] = new JSONValue(_motorAxis.axisFuncName[idx]);
		wstring options = _motorAxis.axisDirName[idx][0];
		if (_motorAxis.axisDirName[idx][1].length() > 0) {
			options += L"|" + _motorAxis.axisDirName[idx][1];
		}
		func[L"options"] = new JSONValue(options);
		func[L"unit"] = new JSONValue(_motorAxis.axisOutputUnit[idx]);
		func[L"is_axial"] = new JSONValue(1);
		slaveFuncs.push_back(new JSONValue(func));
	}
	return new JSONValue(slaveFuncs);
}

//void MotorizedSlave::controlSerialSendCallback(int index, bool success, const void* log, int logLen, const void* data, int dataLen) {
//
//}
//
//void MotorizedSlave::controlSerialReceiveCallback(int index, bool success, const void* log, int logLen, const void* data, int dataLen) {
//
//}

//void MotorizedSlave::initControl() {
	//if (_misc.slave != nullptr) {
	//	auto slv = SlaveNat;
	//	if (SlaveCommVoid != nullptr) {
	//		auto con = SlaveCommNat;
	//		if (SlaveCommNat->isConnected()) {
	//			SlaveCommNat->disconnect();
	//		}
	//		delete SlaveCommVoid;
	//	}
	//	delete _misc.slave;
	//}
	//_misc.slave = new GRBLMaster();
//}

void MotorizedSlave::initDefaultMeta() {
	_meta.referenceName = L"Motor";
	_meta.platform = L"Terrydr";
	_meta.type = L"Motor";
	_meta.model = L"Motor";
	_meta.isExpecting = 1;
	setDefaultFriendlyName();
}

void MotorizedSlave::loadAxisDefinition(GeneralSlave* slave) {
	if (isConnected()) {
		if (_meta.controlFunctions.length() == 0) {
			TLogger::writeLine("No func set after connection. Use default.");
			_meta.controlFunctions = slave->_meta.defaultFunctions;
			_meta.controlParameters = slave->_meta.defaultParameters;
		}
	}
	else {
		_meta.defaultFunctions = slave->_meta.defaultFunctions;
		_meta.defaultParameters = slave->_meta.defaultParameters;
	}
	// bookmark: slaves: motor: parse parameters 
	// bookmark: slaves: pump: parse parameters 
	int axisNum = loadAxisFunctionsCore(_meta.controlFunctions);
	if (axisNum > 0) {
		loadAxisParametersCore(_meta.controlParameters, axisNum);
	}
	//if (_motorAxis.axisFuncName[0].compare(L"pump") == 0) {
	//	_meta.alsoType = L"pump,syringe";
	//}
}

/*
Examples
  "functions":"move:high|low;","parameters":"range=300;"
  "function":"pump(mm):dispense|aspirate;","parameters":"range=50,unit_per_mm=8.33ul;" or "unit_per_mm=1000/60ul"
  "function":"move:left|right;move:back|fore;","parameters":"range=475;range=400;"
*/

/*
Typical set
set functions pump(mm):dispense|aspirate;
*/
int MotorizedSlave::loadAxisFunctionsCore(wstring funcDesp) {
	if (funcDesp.length() == 0) {
		//_motorAxis.flagInit = false;
		return 0;
	}
	for (int idx = 0; idx < _motorAxis.maxAxisNum; idx++) {
		//_motorAxis.axisMask[idx] = 0;
		_motorAxis.axisFuncName[idx] = L"";
		_motorAxis.axisDirName[idx][0] = L"";
		_motorAxis.axisDirName[idx][1] = L"";
		_motorAxis.axisRangeMM[idx] = 0;
		_motorAxis.axisOutputPerCtrlUnit[idx] = 1;
	}
	auto funcs = TFormater::split(funcDesp, L';');
	auto axialFuncs = getAxialFuncs();
	int axisIndex = 0;
	wstring homeDir = L"";
	for (int idx = 0; idx < funcs.size(); idx++) {
		auto funcAxis = funcs[idx];
		if (funcAxis.length() <= 0) {
			continue;
		}
		auto nameAndMethod = TFormater::split(funcAxis, L':');
		if (nameAndMethod.size() != 2) {
			return 0;
		}
		wstring rawName = nameAndMethod[0], name, unit;
		wstring methods = nameAndMethod[1];

		if (rawName.compare(L"home") == 0) {
			// set home direction
			homeDir = methods;
			continue;
		}

		// may include unit in axis name
		if ((rawName.find(L"(") != std::wstring::npos) && (rawName.find(L")") != std::wstring::npos)) {
			auto start = rawName.find(L"("), end = rawName.find(L")");
			unit = rawName.substr(start + 1, end - start - 1);
			name = rawName.substr(0, start);
		}
		else {
			// default init: mm
			unit = L"mm";
			name = rawName;
		}

		bool isFuncAxial = axialFuncs.find(name) != axialFuncs.end();
		if (isFuncAxial) {
			auto dirNames = TFormater::split(methods, L'|');
			_motorAxis.axisFuncName[axisIndex] = name;
			_motorAxis.axisCtrlUnit[axisIndex] = unit;
			for (int dirIdx = 0; dirIdx < dirNames.size(); dirIdx++) {
				_motorAxis.axisDirName[axisIndex][dirIdx] = dirNames[dirIdx];
			}
		}
		else {
			MotorMiscFunctions::MotorMiscFunction func;
			func.name = name;
			func.unit = unit;
			func.calls = TFormater::split(methods, L'|');
			_motorMiscFunctions.funcs.push_back(func);
		}

		if (nameAndMethod.size() >= 3) {
			// duplicate channel config. not necessary as can be set by channel_number parameter
			if (TWFormater::beginWith(nameAndMethod[2], L"#")) {
				// channels share commands
				auto channelNum = TWFormater::str2int(nameAndMethod[2].substr(1));
				for (int chCpIdx = 1; chCpIdx < channelNum; chCpIdx++) {
					_motorAxis.axisFuncName[axisIndex + chCpIdx] = _motorAxis.axisFuncName[axisIndex];
					_motorAxis.axisCtrlUnit[axisIndex + chCpIdx] = _motorAxis.axisCtrlUnit[axisIndex];
					_motorAxis.axisDirName[axisIndex + chCpIdx][0] = _motorAxis.axisDirName[axisIndex][0];
					_motorAxis.axisDirName[axisIndex + chCpIdx][1] = _motorAxis.axisDirName[axisIndex][1];
				}
			}
		}
		axisIndex++;
	}

	if (homeDir.length() > 0) {
		// not available
		if (homeDir.compare(L"NA") == 0) {
			_motorAxis.homeDirection = 0;
		}
		//if (homeDir.compare(_motorAxis.axisDirName[0][1]) == 0) {
		//	_motorAxis.homeDirection = -1;
		//}
		else {
			_motorAxis.homeDirection = 1;
		}
	}

	return funcs.size();
}

/*
Typical set
set parameters range=50,unit_per_mm=8.33ul,channel_number=6;
*/
void MotorizedSlave::loadAxisParametersCore(std::wstring paraDesp, int expectedCount) {
	if (paraDesp.size() == 0) {
		return;
	}
	// axises seperated by ;
	auto paras = TFormater::split(paraDesp, L';');
	if (paras.size() != expectedCount) {
		TLogger::writeWarnning("Motor slave config error: axis function and parameter length not equal");
		TLogger::writeLine(L"Model: " + getModel() + L". Reference: " + getReferenceName());
	}
	_meta.isNormal = 0;

	int chNum = 1;
	for (int idx = 0; idx < paras.size(); idx++) {
		auto paraAxis = paras[idx];
		if (paraAxis.length() <= 0) {
			continue;
		}

		_motorAxis.axisOutputPerCtrlUnit[idx] = 1;
		//_motorAxis.axisUnit[idx] = L'mm';
		// parameter seperated by ,
		auto paraList = TFormater::split(paraAxis, L',');
		for (auto para : paraList) {
			if (TWFormater::beginWith(para, L"range=")) {
				auto rangeStr = para.substr(sizeof("range=") - 1);
				_motorAxis.axisRangeMM[idx] = TWFormater::str2float(rangeStr);
			}
			// multi valve for single pump
			else if (TWFormater::beginWith(para, L"valves")) {
				//TWFormater::append(_motorAxis.axisExFuncCode[idx], para, L",");
				// todo: temp fix
				//para.substr(para.find_last_of();
				MotorMiscFunctions::MotorMiscFunction func;
				func.name = L"valve 0";
				func.calls.push_back(L"0");
				func.calls.push_back(L"1");
				_motorMiscFunctions.funcs.push_back(func);
				func.name = L"valve 3";
				_motorMiscFunctions.funcs.push_back(func);
			}
			else if (TWFormater::beginWith(para, L"valve")) {
				TWFormater::append(_motorAxis.axisExFuncCode[idx], para, L",");
			}
			else if (TWFormater::beginWith(para, L"unit_per_mm=")) {
				auto covStr = para.substr(sizeof("unit_per_mm=") - 1);
				int cIdx = 0;
				for (; cIdx < covStr.length(); cIdx++) {
					if (!TWFormater::charIsElementaryFormula(covStr[cIdx])) {
						break;
					}
				}
				_motorAxis.axisOutputPerCtrlUnit[idx] = TWFormater::formula2float(covStr.substr(0, cIdx));
				_motorAxis.axisOutputUnit[idx] = covStr.substr(cIdx);
			}
			else if (TWFormater::beginWith(para, L"channel_number=")) {
				auto chNumStr = para.substr(sizeof("channel_number=") - 1);
				chNum = TWFormater::str2int(chNumStr);
				_motorAxis.axisCode[0] = L"1";
			}
			else if (TWFormater::beginWith(para, L"ch_num=")) {
				auto chNumStr = para.substr(sizeof("ch_num=") - 1);
				chNum = TWFormater::str2int(chNumStr);
				_motorAxis.axisCode[0] = L"1";
			}
			else if (TWFormater::beginWith(para, L"channels_selectable=")) {
				auto chExpStr = para.substr(sizeof("channels_selectable=") - 1);
				_motorAxis.axisSelectableNum = TWFormater::str2int(chExpStr);
			}
			else if (TWFormater::beginWith(para, L"@")) {
				// preset position, calibrated
				auto presetStr = para.substr(1);
				auto nomimalAndPhysical = TWFormater::split(presetStr, L'=');
				if (nomimalAndPhysical.size() == 2) {
					float norm = TWFormater::str2float(nomimalAndPhysical[0]);
					auto phyStrs = TWFormater::split(nomimalAndPhysical[1], L'|');
					for (int axIdx = 0; axIdx < min(_motorAxis.maxAxisNum, phyStrs.size()); axIdx++) {
						float phy = 0;
						if (phyStrs[axIdx].length() > 0) {
							phy = TWFormater::str2float(nomimalAndPhysical[1]);
						}

						_motorAxis.norm2PhyMaps[axIdx].add(norm, phy);
					}
				}
				_motorAxis.axisCode[0] = L"1";
			}
		}
	}

	int copyChParas = chNum;
	if (_motorAxis.axisSelectableNum > 0) {
		copyChParas = _motorAxis.axisSelectableNum;
	}
	for (int idx = 1; idx < copyChParas; idx++) {
		// start with 1
		_motorAxis.axisCode[idx] = TWFormater::int2str(idx + 1);
		_motorAxis.axisFuncName[idx] = _motorAxis.axisFuncName[0];
		_motorAxis.axisDirName[idx][0] = _motorAxis.axisDirName[0][0];
		_motorAxis.axisDirName[idx][1] = _motorAxis.axisDirName[0][1];
		_motorAxis.axisCtrlUnit[idx] = _motorAxis.axisCtrlUnit[0];
		_motorAxis.axisOutputUnit[idx] = _motorAxis.axisOutputUnit[0];
		_motorAxis.axisOutputPerCtrlUnit[idx] = _motorAxis.axisOutputPerCtrlUnit[0];
		_motorAxis.axisRangeMM[idx] = _motorAxis.axisRangeMM[0];
		_motorAxis.axisExFuncCode[idx] = _motorAxis.axisExFuncCode[0];
	}

	_motorAxis.flagInit = true;
}

void MotorizedSlave::updateAfterConnectAny(HandShakeProtocol* protocol) {
	GeneralSlave::updateAfterConnectAny(protocol);
	if (!_motorAxis.flagInit) {
		// not configured staticly, must load dynamically
		if (_comm.commHandle->sendString("functions\n") > 0) {
			auto funcFromSlave = TFormater::string2wstring(_comm.commHandle->receiveStrReplyWithDelim(1000, '\n'));
			funcFromSlave = TWFormater::removeDelims(funcFromSlave, L"\r\n");
			if (funcFromSlave.length() <= 1) {
				loadAxisDefinition(this);
			}
			else {

				int axisNum = loadAxisFunctionsCore(funcFromSlave);

				if (_comm.commHandle->sendString("parameters\n") > 0) {
					auto parasFromSlave = TFormater::string2wstring(_comm.commHandle->receiveStrReplyWithDelim(1000, '\n'));
					parasFromSlave = TWFormater::removeDelims(parasFromSlave, L"\r\n");
					loadAxisParametersCore(parasFromSlave, axisNum);
				}
			}
		}
	}
}

//void MotorizedSlave::updateAfterConnectAnyMotor() {
	//if (!_motorAxis.flagInit) {
	//	// not configured staticly, must load dynamically
	//	if (_comm.commHandle->sendString("functions\n") > 0) {
	//		auto funcFromSlave = TFormater::string2wstring(_comm.commHandle->receiveStrReplyWithDelim(1000, '\n'));
	//		funcFromSlave = TWFormater::removeDelims(funcFromSlave, L"\r\n");
	//		int axisNum = loadAxisFunctionsCore(funcFromSlave);

	//		if (_comm.commHandle->sendString("parameters\n") > 0) {
	//			auto parasFromSlave = TFormater::string2wstring(_comm.commHandle->receiveStrReplyWithDelim(1000, '\n'));
	//			parasFromSlave = TWFormater::removeDelims(parasFromSlave, L"\r\n");
	//			loadAxisParametersCore(parasFromSlave, axisNum);
	//		}
	//	}
	//}
//}

std::set<std::wstring> MotorizedSlave::getAxialFuncs() {
	std::set<std::wstring> ret;

	ret.insert(L"move");

	return ret;
}
